/* 大疆 M2006 + C610 驱动程序 */
#include <Arduino.h>
#include "driver/gpio.h"
#include "driver/twai.h"

// Duty 为百分比，例：5 == 5% == 5%*10000 == 500
int Duty = 5;
// 控制转矩电流值范围  -10000 ~ 0 ~ 10000
int current = 500;
// 每个元素是8位
char stmp[8] = {0, 0, 0, 0, 0, 0, 0, 0};

/* TWAI(CAN) 初始化函数
 * TX ---> GPIO_NUM_5
 * RX ---> GPIO_NUM_4
 * BaudRate ---> 1MBITS
 */
void TWAI_Init()
{
    // 初始化配置引脚，频率和滤波器
    twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_5, GPIO_NUM_4, TWAI_MODE_NORMAL);
    twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();
    twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

    // 将配置添加到twai驱动中
    if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK)
        printf("Driver installed\n");
    else
    {
        printf("Failed to install driver\n");
        return;
    }

    // 开始运行twai
    if (twai_start() == ESP_OK)
        printf("Driver started\n");
    else
    {
        printf("Failed to start driver\n");
        return;
    }
}

// TWAI(CAN)发送数据函数
void TWAI_Send_Data()
{
    // 定义发送数据帧内容
    twai_message_t message;
    message.identifier = 0x200;
    message.extd = 0;
    message.data_length_code = 8;

    // TWAI 数据发送
    message.data[0] = stmp[0];
    message.data[1] = stmp[1];
    message.data[2] = stmp[2];
    message.data[3] = stmp[3];
    message.data[4] = stmp[4];
    message.data[5] = stmp[5];
    message.data[6] = stmp[6];
    message.data[7] = stmp[7];

    // 调用发送数据函数
    if (twai_transmit(&message, pdMS_TO_TICKS(1000)) == ESP_OK)
        printf("Message Send Success!\n");
    else
        printf("Failed To Send Message...\n");
}

// TWAI(CAN)接收数据函数
void TWAI_Receive_Data()
{
    // Wait for message to be received
    twai_message_t message;
    if (twai_receive(&message, pdMS_TO_TICKS(5000)) == ESP_OK)
        printf("Message received\n");
    else
    {
        printf("Failed To Receive Message\n");
        return;
    }

    // Process received message
    if (message.extd)
        printf("Message is in Extended Format\n");
    else
    {
        printf("Message is in Standard Format\n");
    }
    printf("ID is %d\n", message.identifier);
    if (!(message.rtr))
        for (int i = 0; i < message.data_length_code; i++)
            printf("Data byte %d = %d\n", i, message.data[i]);
}

// 速度控制
void Speed_Control(int duty_set)
{
    current = Duty * 100;
    stmp[0] = (current >> 8); // 电流控制 高八位
    stmp[1] = current;        // 电流控制 低八位
}

void setup()
{
    TWAI_Init();
}

void loop()
{
    Speed_Control(5);
    TWAI_Send_Data();
    TWAI_Receive_Data();
    delay(1000);
}